%% functions
function [Output] = makeblkdiag(n,A,B,C)
% function that makes a TDM for Q
% combines multiple matrix A,B,C into one TDM
    Output = zeros(n^2,n^2);
    for i = 1:n:n^2
        for j = i:n:n^2
            Output(i:i+n-1,j:j+n-1) = B;
            break;
        end
    end
    for i = 1:n:n^2-n
        for j = i:n:n^2-n
            x = i+n;
            Output(x:x+n-1,j:j+n-1) = A;
            break;
        end
    end
    for i = 1:n:n^2-n
        for j = i:n:n^2-n
            y = i+n;
            Output(i:i+n-1,y:y+n-1) = C;
            break;
        end
    end
end
